
# !处理路径导入问题（添加绝对路径）！！！
import sys
import os
CODE_INTERNAL_PATH = os.path.abspath(os.path.join(os.path.dirname(__file__), '../../')) # 生成Code文件夹内部对应的绝对路径
sys.path.append(CODE_INTERNAL_PATH)

import numpy as np
import matplotlib.pyplot as plt

from kalman_filter._02_ekf_bicyle_model import BicycleModelKalmanFilter

plt.rcParams["font.sans-serif"] = ["SimHei"]
plt.rcParams["axes.unicode_minus"] = False
plt.rc("font", size=15)

def simulate_vehicle_motion(duration, dt):
    """
    模拟车辆运动生成测试数据
    """
    time_steps = int(duration / dt)
    t = np.linspace(0, duration, time_steps)
    
    # 真实状态
    true_states = np.zeros((time_steps, 4))
    # 初始状态
    true_states[0] = [0, 0, 5, 0]  # [x, y, v, theta]

    # 控制输入: [加速度, 转向角]
    controls = np.zeros((time_steps, 2))
    
    # 生成控制序列
    for i in range(time_steps):
        # 周期性控制输入
        controls[i, 0] = 0.5 * np.sin(0.5 * t[i])  # 加速度
        controls[i, 1] = 0.2 * np.sin(0.8 * t[i])  # 转向角
    
    # 模拟车辆运动
    for i in range(1, time_steps):
        # 简单的欧拉积分
        x, y, v, theta = true_states[i-1]
        a, delta = controls[i-1]
        
        v_new = v + a * dt
        theta_new = theta + (v / 2.5) * np.tan(delta) * dt
        x_new = x + v_new * np.cos(theta_new) * dt
        y_new = y + v_new * np.sin(theta_new) * dt
        
        true_states[i] = [x_new, y_new, v_new, theta_new]
    
    return t, true_states, controls

def add_noise(data, stds_dev, noise_type="gaussian"):
    """添加噪声到数据"""
    stds_dev = np.array(stds_dev)
    if noise_type == "gaussian":
        # 高斯噪声
        noise = np.random.normal(0, stds_dev, data.shape)
    elif noise_type == "uniform":
        # 均匀噪声范围为 [-std, std]
        noise = np.random.uniform(-stds_dev, stds_dev, data.shape)
    return data + noise

def compare_state(true_states, measurements):
    for i in range(int(true_states.shape[0] / 5)):
        print("x状态: ", true_states[i, 0], measurements[i, 0])
        print("y状态: ", true_states[i, 1], measurements[i, 1])
        print("v状态: ", true_states[i, 2], measurements[i, 2])
        print("theta状态: ", true_states[i, 3], measurements[i, 3])
        print("")

# 测试和演示
if __name__ == "__main__":
    # 参数设置
    dt = 0.1  # 时间步长
    duration = 10  # 模拟时长

    # 生成测试数据
    t, true_states, controls = simulate_vehicle_motion(duration, dt)

    # 添加噪声创建测量值
    measurement_noise_std = [0.8, 0.8, 0.8, 0.1]
    measurements = add_noise(true_states, measurement_noise_std, noise_type="uniform")
    compare_state(true_states, measurements)

    # 创建卡尔曼滤波器
    kf = BicycleModelKalmanFilter(dt=dt, wheelbase=2.5, 
                                 process_noise_std=[0.3, 0.3, 0.05, 0.02], 
                                 measurement_noise_std=measurement_noise_std)

    # 设置初始状态
    kf.set_state(measurements[0])

    # 存储估计结果
    estimated_states = np.zeros_like(true_states)
    estimated_states[0] = kf.get_state()

    # 存储每个状态的协方差
    estimated_covariances = np.zeros((len(t), kf.state_dim))

    # 运行卡尔曼滤波
    for i in range(1, len(t)):
        # 预测步骤
        kf.predict(controls[i-1])
        
        # 更新步骤
        kf.update(measurements[i])
        
        # 存储结果
        estimated_states[i] = kf.get_state()

        # 保存协方差信息
        P = kf.get_covariance()
        estimated_covariances[i] = [P[0,0], P[1,1], P[2,2], P[3,3]]  # 只存储对角线元素（方差）

    # 绘制结果
    plt.figure(figsize=(16, 10))
    
    # 轨迹图
    plt.subplot(3, 3, 1)
    plt.plot(true_states[:, 0], true_states[:, 1], 'g-', label='真实轨迹', linewidth=2)
    plt.plot(measurements[:, 0], measurements[:, 1], 'ro', markersize=2, alpha=0.5, label='测量值')
    plt.plot(estimated_states[:, 0], estimated_states[:, 1], 'b-', label='估计轨迹', linewidth=2)
    plt.xlabel('X位置 (m)')
    plt.ylabel('Y位置 (m)')
    plt.title('车辆轨迹')
    plt.legend()
    plt.grid(True)
    plt.axis('equal')
    
    # X位置随时间变化
    plt.subplot(3, 3, 2)
    plt.plot(t, true_states[:, 0], 'g-', label='真实值', linewidth=2)
    plt.plot(t, measurements[:, 0], 'ro', markersize=2, alpha=0.5, label='测量值')
    plt.plot(t, estimated_states[:, 0], 'b-', label='估计值', linewidth=1)
    plt.xlabel('时间 (s)')
    plt.ylabel('X位置 (m)')
    plt.title('X位置')
    plt.legend()
    plt.grid(True)
    
    # Y位置随时间变化
    plt.subplot(3, 3, 3)
    plt.plot(t, true_states[:, 1], 'g-', label='真实值', linewidth=2)
    plt.plot(t, measurements[:, 1], 'ro', markersize=2, alpha=0.5, label='测量值')
    plt.plot(t, estimated_states[:, 1], 'b-', label='估计值', linewidth=1)
    plt.xlabel('时间 (s)')
    plt.ylabel('Y位置 (m)')
    plt.title('Y位置')
    plt.legend()
    plt.grid(True)
    
    # 速度随时间变化
    plt.subplot(3, 3, 4)
    plt.plot(t, true_states[:, 2], 'g-', label='真实值', linewidth=2)
    plt.plot(t, measurements[:, 2], 'ro', markersize=2, alpha=0.5, label='测量值')
    plt.plot(t, estimated_states[:, 2], 'b-', label='估计值', linewidth=1)
    plt.xlabel('时间 (s)')
    plt.ylabel('速度 (m/s)')
    plt.title('速度')
    plt.legend()
    plt.grid(True)
    
    # 航向角随时间变化
    plt.subplot(3, 3, 5)
    plt.plot(t, np.degrees(true_states[:, 3]), 'g-', label='真实值', linewidth=2)
    plt.plot(t, np.degrees(measurements[:, 3]), 'ro', markersize=2, alpha=0.5, label='测量值')
    plt.plot(t, np.degrees(estimated_states[:, 3]), 'b-', label='估计值', linewidth=1)
    plt.xlabel('时间 (s)')
    plt.ylabel('航向角 (度)')
    plt.title('航向角')
    plt.legend()
    plt.grid(True)
    
    # 位置误差
    plt.subplot(3, 3, 6)
    position_error = np.sqrt((estimated_states[:, 0] - true_states[:, 0])**2 + 
                            (estimated_states[:, 1] - true_states[:, 1])**2)
    plt.plot(t, position_error, 'r-', linewidth=1)
    plt.xlabel('时间 (s)')
    plt.ylabel('位置误差 (m)')
    plt.title('位置估计误差')
    plt.grid(True)

    # 每个状态的协方差
    plt.subplot(3, 3, 7)
    plt.plot(t, estimated_covariances[:, 0], label='横向位置', linewidth=1)
    plt.plot(t, estimated_covariances[:, 1], label='纵向速度', linewidth=1)
    plt.plot(t, estimated_covariances[:, 2], label='轴向速度', linewidth=1)
    plt.plot(t, estimated_covariances[:, 3], label='航向角', linewidth=1)
    plt.xlabel('时间 (s)')
    plt.ylabel('方差')
    plt.title('状态方差')
    plt.legend()
    plt.grid(True)

    plt.tight_layout()
    plt.show()
    
    # 打印统计信息
    final_position_error = position_error[-1]
    avg_position_error = np.mean(position_error)
    print(f"最终位置误差: {final_position_error:.3f} m")
    print(f"平均位置误差: {avg_position_error:.3f} m")
    print(f"最大位置误差: {np.max(position_error):.3f} m")